#include "CatmullRomCurveEvaluator.h"
#include <assert.h>

void CatmullRomCurveEvaluator::evaluateCurve(const std::vector<Point>& ptvCtrlPts, 
										 std::vector<Point>& ptvEvaluatedCurvePts, 
										 const float& fAniLength, 
										 const bool& bWrap) const
{
	
	ptvEvaluatedCurvePts.clear();
	
	if (!bWrap) {
		// Add in first point at the start of the animation
		ptvEvaluatedCurvePts.push_back(Point(0, ptvCtrlPts[0].y)); 

		// Create splines and duplicate end points as necessary
		for (int x = -1; x+2 < (int)ptvCtrlPts.size(); x++) {
			vector<Point> points = getSingleSpline(ptvCtrlPts[max(0, x)], ptvCtrlPts[x+1], ptvCtrlPts[x+2], ptvCtrlPts[min((int)ptvCtrlPts.size()-1, x+3)]);
			for (unsigned int y = 0; y < points.size(); y++) ptvEvaluatedCurvePts.push_back(points[y]);
		}

		// Add in final point at the end of the animation
		ptvEvaluatedCurvePts.push_back(Point(fAniLength, ptvCtrlPts.back().y));
	} else {

		vector<Point> tempPoints;

		// Create splines, excluding endpoints - note special treatment for fake endpoints
		for (int x = 0; x < (int)ptvCtrlPts.size(); x++) {
			vector<Point> points = getSingleSpline(ptvCtrlPts[x], 
												   Point(ptvCtrlPts[(x+1) % ptvCtrlPts.size()].x + (x+1)/ptvCtrlPts.size() * fAniLength, ptvCtrlPts[(x+1) % ptvCtrlPts.size()].y),
												   Point(ptvCtrlPts[(x+2) % ptvCtrlPts.size()].x + (x+2)/ptvCtrlPts.size() * fAniLength, ptvCtrlPts[(x+2) % ptvCtrlPts.size()].y),
												   Point(ptvCtrlPts[(x+3) % ptvCtrlPts.size()].x + (x+3)/ptvCtrlPts.size() * fAniLength, ptvCtrlPts[(x+3) % ptvCtrlPts.size()].y));
			for (unsigned int y = 0; y < points.size(); y++) { 
				if (points[y].x >= fAniLength) {
					ptvEvaluatedCurvePts.push_back(Point(fmod(points[y].x, fAniLength), points[y].y));
				} else {
					tempPoints.push_back(points[y]);
				}
			}
		}

		for (vector<Point>::iterator i = tempPoints.begin(); i != tempPoints.end(); i++) {
			ptvEvaluatedCurvePts.push_back(*i);
		}
		
	}

	ptvEvaluatedCurvePts = trimPoints(ptvEvaluatedCurvePts);
	
}

vector<Point> CatmullRomCurveEvaluator::getSingleSpline(Point b0, Point b1, Point b2, Point b3) const {

	// Catmullrom basis matrix
	Mat4<double> basis(-1, 3, -3, 1, 2, -5, 4, -1, -1, 0, 1, 0, 0, 2, 0, 0); 
	basis = basis/2;
	basis = basis.transpose(); 

	Vec4<double> ctrl_x(b0.x, b1.x, b2.x, b3.x);
	Vec4<double> ctrl_y(b0.y, b1.y, b2.y, b3.y);
	
	return evaluateSpline(basis, ctrl_x, ctrl_y, CATMULLROM_PARAMETER_SAMPLE);
}
